COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	{$NOMAIN	Main "MOVE" statement interpreter }
C00025 00003	{ Externally defined routines: }
C00030 00004	procedure doMove external
C00033 00005	 procedure doMode1
C00040 00006	 procedure doMode2
C00051 00007	 begin {doMove - main body}
C00053 ENDMK
C⊗;
{$NOMAIN	Main "MOVE" statement interpreter }

const

	YELARM = 1B;		(* Yellow arm *)
	BLUARM = 4B;		(* Blue arm *)

	FTABLE = 400B;		(* Force trans (C) in table coordinates *)
	FHAND  = 0B;		(*   "	 "    "   " hand coordinate system *)

	nullingcb = 1B;		(* control bits for trajectory specs *)
	wobblecb = 2B;
	durlbcb = 20B;		(* Duration: lower, upper & exact bounds *)
	durubcb = 40B;
	dureqcb = 60B;
	veloccb = 100B;
	codecb = 200B;
	viaptcb = 400B;
	deprptcb = 1000B;
	apprptcb = 2000B;
	destptcb = 10000B;

	(* Constants from EDIT *)
      	maxLines = 28;		(* smaller on the 11 than on the 10 *)
      	maxPPLines = 18;
      	maxBpts = 25;
      	maxTBpts = 20;	(* max could be exceeded by huge case stmnt *)
	listinglength = 2000;	(* Length of Listingarray *)



type

  byte = 0..255;	(* doesn't really belong here, but... *)
  ascii = char; 
  atext = text;

{ Define all the pointer types here }

vectorp = ↑vector;
transp = ↑trans;
strngp = ↑strng;
eventp = ↑event;
framep = ↑frame;
identp = ↑ident;
varidefp = ↑varidef;
nodep = ↑node;
pdbp = ↑pdb;
statementp = ↑statement;
envheaderp = ↑envheader;
enventryp = ↑enventry;
environp = ↑environment;
messagep = ↑message;
cmoncbp = ↑cmoncb;

dump = ↑integer;
token = array[1..4] of integer;	{Same size as a token}
cursorp = array[1..4] of integer;


(* datatype definitions *)

datatypes = (pconstype, varitype, svaltype, vectype, rottype, transtype,
	     frametype, eventtype, strngtype, labeltype, proctype, arraytype,
	     reftype, valtype, cmontype, nulltype, undeftype,
	     dimensiontype, mactype, macargtype, freevartype);

scalar = real;

u = (used,free);
vector = record case u of
	   used: (refcnt: integer; val: array [1..3] of real);
	   free: (next: vectorp);
	 end;

trans = record case u of
	   used: (refcnt: integer; val: array [1..3,1..4] of real);
	   free: (next: transp);
	end;

cstring = packed array [1..10] of ascii;
c4str = packed array [1..4] of ascii;
c5str = packed array [1..5] of ascii;
c20str = packed array [1..20] of ascii;
linestr = packed array [1..130] of ascii;

strng = record
	  next: strngp;
	  ch: cstring;
	end;


event = record
	  next: eventp;		(* all events are on one big list *)
	  count: integer;
	  waitlist: pdbp;
	end;


frame = record
	  vari: varidefp;	(* back pointer to variable name & info *)
	  calcs: nodep;		(* affixment info *)
	  case ftype: boolean of	(* frame = true, device = false *)
  true:	    (valid: integer; val, fdepr: transp; dcntr: integer; dev: framep);
  false:    (mech: integer; case sdev: boolean of
		true: (sdest: real); false: (tdest,appr,depr: transp));
		(* sdev = true for scalar devices, false for frames *)
	end;




(* statement definitions *)

stmntypes = (progtype, blocktype, coblocktype, endtype, coendtype,
		fortype, iftype, whiletype, untiltype, casetype,
		calltype, returntype,
		printtype, prompttype, pausetype, aborttype, assigntype,
		signaltype, waittype, enabletype, disabletype, cmtype,
		affixtype, unfixtype,
		movetype, operatetype, opentype, closetype, centertype,
		stoptype, retrytype,
		requiretype, definetype, macrotype, commenttype, dimdeftype,
		setbasetype, wristtype, tovaltype, declaretype, emptytype);
		(* more??? *)

statement = packed record
		next, last: statementp; (* ↑ to lexical tokens? *)
		stlab: varidefp;
		exprs: nodep;	(* any expressions used by this statement *)
		nlines: integer;
		bpt: boolean;
		case stype: stmntypes of

    movetype,
    operatetype,
    opentype,
    closetype,
    centertype,
    stoptype:	    (cf, clauses: nodep);
    waittype:	    (event: nodep);
    retrytype:	    (rcode, rparent: statementp; olevel: integer);
    cmtype:	    (oncond: nodep; conclusion: statementp;
			deferCm, exprCm: boolean; cdef: varidefp);
		end;


(* auxiliary definitions: variable, etc. *)

varidef = packed record
	    next,dnext: varidefp;
	    name: identp;
	    level: 0..255;	(* environment level *)
	    offset: 0..255;	(* environment offset *)
	    dtype: varidefp;	(* to hold the dimension info *)
	    tbits: 0..15;  (* special type bits: array = 1, proc = 2, ref = 4 & ? *)
	    dbits: 0..15;	(* for use by debugger/interpreter *)
	    case vtype: datatypes of
  arraytype:  (a: nodep);
  proctype:   (p: nodep);
  labeltype,
  cmontype:   (s: dump);
  mactype:    (mdef: dump);
  macargtype: (marg: dump);
  pconstype:  (c: nodep);
  dimensiontype: (dim: nodep);
	  end;




(* definition of the ubiquitous NODE record *)

nodetypes = (exprnode, leafnode, listnode, clistnode, colistnode, forvalnode,
		deprnode, viaptnode, apprnode, destnode, durnode,
		sfacnode, wobblenode, swtnode, nullingnode, wristnode, cwnode,
		arrivalnode, departingnode,
		ffnode, forcenode, stiffnode, gathernode, cmonnode, errornode,
		calcnode, arraydefnode, bnddefnode, bndvalnode,
		waitlistnode, procdefnode, tlistnode, dimnode, commentnode);

exprtypes =  (	svalop,					(* scalar operators *)
		sltop, sleop, seqop, sgeop, sgtop, sneop,	(* relations *)
		notop, orop, xorop, andop, eqvop,		(* logical *)
		saddop, ssubop, smulop, sdivop, snegop, sabsop, (* scalar ops *)
		sexpop, maxop, minop, intop, idivop, modop,
		sqrtop, logop, expop, timeop,			(* functions *)
		sinop, cosop, tanop, asinop, acosop, atan2op,	(* trig *)
		vdotop, vmagnop, tmagnop,
		vecop,					(* vector operators *)
		vmakeop, unitvop, vaddop, vsubop, crossvop, vnegop,
		svmulop, vsmulop, vsdivop, tvmulop, wrtop,
		tposop, taxisop,
		transop,				(* trans operators *)
		tmakeop, torientop, ttmulop, tvaddop, tvsubop, tinvrtop,
		vsaxwrop, constrop, ftofop, deproachop, fmakeop, vmkfrcop,
		ioop,					(* i/o operators *)
		queryop, inscalarop,
		specop,					(* special operators *)
		arefop, callop, grinchop, macroop, vmop, adcop, dacop,
		badop,
		addop, subop, negop, mulop, divop, absop); (* for parsing *)

leaftypes = pconstype..strngtype;

reltypes = sltop..sgtop;
forcetypes = (force,absforce,torque,abstorque,angvelocity);

node = record
	next: nodep;
	case ntype: nodetypes of
    exprnode:	(op: exprtypes; arg1, arg2, arg3: nodep; elength: integer);
    leafnode:	(case ltype: leaftypes of
	varitype:  (vari: varidefp; vid: dump);
	pconstype: (cname: varidefp; pcval: nodep);
	svaltype:  (s: scalar; wid: integer);
	vectype:   (v: vectorp);
	transtype: (t: transp);
	strngtype: (length: integer; str: strngp) ); (* also used by commentnodes *)
    listnode:	(lval: nodep);
    clistnode:	(cval: integer; stmnt: dump; clast: nodep);
    colistnode:	(prev: nodep; cstmnt: dump);
    forvalnode:	(fvar: enventryp; fstep: scalar);
    arrivalnode:(evar: varidefp);
    deprnode,
    apprnode,
    destnode:	(loc: nodep; code: dump);
    viaptnode:	(vlist: boolean; via,duration,velocity: nodep; vcode: dump);
    durnode:	(durrel: reltypes; durval: nodep);
    sfacnode,
    wobblenode,
    swtnode:	(clval: nodep);
    nullingnode,
    wristnode,
    cwnode:	(notp: boolean); (* true = nonulling/zero wrist/counter_clockwise *)
    ffnode:	(ff: nodep; csys, pdef: boolean); (* true = world, false = hand *)
    forcenode:	(ftype: forcetypes; frel: reltypes; fval, fvec, fframe: nodep);
    stiffnode:	(fv, mv, coc: nodep);
    gathernode:	(gbits: integer);
    cmonnode:	(cmon: statementp; errhandlerp: boolean);
    errornode:	(eexpr: nodep);
    calcnode: 	(rigid, frame1: boolean; other: framep; case tvarp: boolean of 
		    false: (tval: transp); true: (tvar: enventryp) );
    arraydefnode: (numdims: 1..10; bounds: nodep; combnds: boolean);
    bnddefnode:	(lower, upper: nodep);
    bndvalnode:	(lb, ub, mult: integer);
    waitlistnode: (who: pdbp; when: integer);
    procdefnode:(ptype: datatypes; level: 0..255;
		    pname, paramlist: varidefp; body: dump);
    tlistnode:	(tok: dump);
    dimnode:	(time, distance, angle, dforce: integer);
	end;





(* process descriptor blocks & environment record definitions *)

queuetypes = (nullqueue,nowrunning,runqueue,inputqueue,eventqueue,sleepqueue,
		forcewait,devicewait,joinwait,proccall);

pdb = packed record
	nextpdb,next: pdbp;	(* for list of all/active pdb's *)
	level: 0..255;		(* lexical level *)
	mode: 0..255;		(* expression/statement/sub-statement *)
	priority: 0..255;
	status: queuetypes;	(* what are we doing *)
	env: envheaderp;
	spc: statementp;	(* current statement *)
	epc: nodep;		(* current expression (if any) *)
	sp: nodep;		(* intermediate value stack *)
	cm: cmoncbp;		(* if we're a cmon point to our definition *)
	mech: framep;		(* current device being used *)
	linenum: integer;	(* used by editor/debugger *)
	 case procp: boolean of	(* true if we're a procedure *)
true:  (opdb: pdbp;		(* pdb to restore when procedure exits *)
	pdef: nodep);		(* procedure definition node *)
false: (evt: eventp;		(* event to signal when process goes away *)
	sdef: dump);	(* first statement where process was defined *)
      end;


envheader = packed record
	      parent: envheaderp;
	      env: array [0..4] of environp;
	      varcnt: 0..255;		(* # of variables in use ??? *)
		case procp: boolean of  (* true if we're a procedure *)
	true: (proc: nodep);
	false:(block: dump);
	    end;


enventry = record
	    case etype: datatypes of
  svaltype:  (s: scalar);
  vectype:   (v: vectorp);
  transtype: (t: transp);
  frametype: (f: framep);
  eventtype: (evt: eventp);
  strngtype: (length: integer; str: strngp);
  cmontype:  (c: cmoncbp);
  proctype:  (p: nodep; penv: envheaderp);
  reftype:   (r: enventryp);
  arraytype: (a: envheaderp; bnds: nodep);
	   end;


environment = record
		next: environp;
		vals: array [0..9] of enventryp;
	      end;


cmoncb = record
	   running, enabled: boolean;		(* cmon's status *)
	   cmon: dump;
	   pdb: pdbp;
	   evt: eventp;
	   fbits: integer;			(* bits for force sensing *)
	   oldcmon: cmoncbp;			(* for debugger *)
	 end;

ident = record
	    next: identp;
	    length: integer;
	    name: strngp;
	    predefined: varidefp;
	  end;



(* definition of AL-ARM messages *)

msgtypes = (initarmscmd,calibcmd,killarmscmd,wherecmd,
	    abortcmd,stopcmd,movehdrcmd,movesegcmd,
	    centercmd,operatecmd,movedonecmd,signalcmd,readjtcmd,drivecmd,
	    setccmd,forcesigcmd,forceoffcmd,biasoncmd,biasoffcmd,setstiffcmd,
	    zerowristcmd,wristcmd,gathercmd,getgathercmd,readadccmd,writedaccmd,
	    errorcmd,floatcmd);

errortypes = (noerror,noarmsol,timerr,durerr,toolong,useopr,nosuchdev,featna,
	      unkmess,srvdead,adcdead,nozind,exjtfc,paslim,nopower,badpot,devbusy,
	      baddev,timout,panicb);

message = record
	   cmd: msgtypes;
	   ok: boolean;
	   case integer of
	1:   (dev, bits, n: integer;
(*	     (dev, bits, n, evt: integer;	(* for arm code version *)
	      evt: eventp;
	      dur: real;
	      case integer of
		1: (v1,v2,v3: real);
		2: (sfac,wobble,pos: real);
		3: (val,angle,mag: real);
		4: (max,min: real);
		5: (error: errortypes));
	2:   (fv1,fv2,fv3,mv1,mv2,mv3: real);	(* may never use these... *)
	3:   (t: array [1..6] of real);
	  end;

interr = record
         case integer of
           0: (i: integer);
	   1: (err,foo: errortypes);
	 end;



(* Global variables *)

var 
	(* From ALMAIN *)
    b:boolean;		
    ch:ascii;
    ltime: real;

	(* From PARSE *)
    reswords: array [0..26] of dump;
    idents: array [0..26] of dump;
    macrostack: array [1..10] of dump;
    curmacstack: array [1..10] of varidefp;
    macrodepth: integer;
    curchar, maxchar, curline: integer;
    curBlock,newDeclarations: dump;
    curProc: varidefp;
    pnode: nodep;
    nodim, distancedim, timedim, angledim,
      forcedim, torquedim, veldim, angveldim: varidefp;
    fvstiffdim, mvstiffdim: nodep;
    filedepth: integer;
    curpage: integer;
    sysVars,unVars: varidefp;
    errcount: integer;
    outerBlock: dump;
    curVariable: varidefp;
    curMotion: dump;
    endOk,coendOk: integer;
    moveLevel: integer;
    curErrhandler, curCmon: dump;
    endOfLine, backup, expandmacros, flushcomments, dimCheck: boolean;
    semiseen, shownline: boolean;
    eofError: boolean;
    inMove,inCoblock: boolean;
    curtoken: token;	
    file1,file2,file3,file4,file5: atext;
    line: linestr;

	(* From INTERP *)
    curInt, activeInts, readQueue, allPdbs: pdbp;
    curEnv, sysEnv: envheaderp;
    clkQueue: nodep;
    allEvents: eventp;
    STLevel: integer;		(* set by GO *)
    etime: integer;		(* used by eval *)
    curtime: integer; 		(* Time of day, in ticks *)
    stime: integer;		(* used for clock queue on 10 *)
    msg: messagep;		(* for AL-ARM interaction *)
    inputp: integer;		(* current offset into inputLine array above *)
    debugLevel: integer;
    tSingleThreadMode: boolean;
    resched, running, escapeI, singleThreadMode: boolean;
    msgp: boolean;		(* flag set if any messages pending *)
    inputReady: boolean;
    inputLine: array [1..20] of ascii;

	(* From EDIT *)
    lines: array [1..maxLines] of dump; 
    ppLines: array [1..maxPPLines] of dump;	
    marks: array [1..20] of integer;
    cursorStack: array [1..15] of cursorp;
    bpts: array [1..maxBpts] of dump;
    tbpts: array [1..maxTBpts] of dump;
    debugPdbs: array [0..10] of pdbp;
    screenheight,dispHeight: integer;
    ppBufp,oppBufp,ppOffset,ppSize,nmarks: integer;
    lbufp,cursor,ocur,cursorLine,fieldnum,lineNum,findLine,pcLine: integer;
    firstDline,topDline,botDline,firstLine,lastLine: integer;
    freeLines,oldLines: dump;
    findStmnt: dump;
    nbpts,ntbpts: integer;
    eCurInt: pdbp;
    dProg: dump;
    smartTerminal: boolean; 
    setUp,setExpr,setCursor,dontPrint,outFilep,newVarOk,collect: boolean;
    eBackup: boolean;			
    eSingleThreadMode: boolean;	
    listing: packed array [0..listinglength] of ascii;
    lbuf: array [1..160] of ascii;
    ppBuf: array [1..100] of ascii;
    outFile: atext;
    eCurToken: token;	

	(* Various device & variable pointers *)
    speedfactor: enventryp;
    barm: framep;

	(* Various constant pointers *)
    xhat,yhat,zhat,nilvect: vectorp;
    niltrans: transp;
    bpark, ypark, gpark, rpark: transp;		(* arm park positions *)

{ Externally defined routines: }

	(* From ALLOC *)
procedure relTrans(t: transp);					external;
procedure relNode(n: nodep);					external;

	(* From IAUX1A *)
procedure upTrans (var t: transp; tp: transp);			external;
function getVar (level, offset: byte): enventryp;		external;
function gtVarn (n: nodep): enventryp;				external;
function getNval(n: nodep; var b: boolean): nodep;		external;
function getEvent: eventp;					external;
procedure freeEvent(e: eventp);					external;
procedure killNode(n: nodep);					external;
procedure sendCmd;						external;
procedure sendTrans(tr: transp);				external;
function feval (f: framep): transp;				external;

	(* From RSXMSG *)
procedure signalArm;                                         	external;

	(* Arithmetic Routines *)
function ttmul (t1,t2: transp): transp; 			external;
function tinvrt (t: transp): transp; 				external;
function svmul (s: scalar; v: vectorp): vectorp; 		external;
function tmake (t: transp; v: vectorp): transp; 		external;
function tvadd (t: transp; v: vectorp): transp; 		external;

	(* From IAUX2A *)
function whereArm (mech: integer): transp;			external;

	(* From IROOT *)
function m1Forcebits(fn: nodep; var negv: boolean): integer;	external;
function m1GetMechbits: integer;				external;
procedure m1MvStart;						external;
procedure m1MvEnd;						external;
procedure m1MvRetry;						external;

	(* Display-related Routines *)
procedure ppLine; 						external;
procedure ppOutNow; 						external;
procedure ppChar(ch: ascii); 					external;
procedure pp5(ch: c5str; length: integer); 			external;
procedure pp10(ch: cstring; length: integer); 			external;
procedure pp10L(ch: cstring; length: integer);			external;
procedure pp20(ch: c20str; length: integer); 			external;
procedure pp20L(ch: c20str; length: integer); 			external;
procedure ppInt(i: integer); 					external;
procedure ppReal(r: real); 					external;
procedure ppStrng(length: integer; s: strngp); 			external;


procedure iMove1Get; external;
procedure iMove1Get; begin end;
procedure doMove; external;

(* This fella has been changed also from the original version on the 10,	*)
(* since the procedure was too big to optimize, etc.  The 2 big parts of	*)
(* the main CASE stmnt have been made into procedures.				*)

procedure doMove;
 var appr,depr,dest,arrv,wobble,sfac,dur,ffr,stiff,gather,zwrist,n: nodep;
     cl,val,val1,val2: nodep; t,tl,tb: transp; st: statementp; e: enventryp;
     r: real; fbits,nsegs,mechbits,i,cmForce,useForce: integer; fr: framep;
     b,b1,b2,nulling,apprp,deprp: boolean; ev: eventp;

 function getLoc(n: nodep): transp;
  var tp: transp; b: boolean;
  begin
  n := getNval(n,b);
  tp := n↑.t;
  if b then relNode(n);
  if t <> nil then tp := ttmul(t,tp);
  getLoc := tp;
  end;

 function getDepr(n: nodep; b: boolean): transp;
  var tp: transp; v: vectorp;
  begin
  if n↑.ltype = svaltype then tp := tmake(niltrans,svmul(n↑.s,zhat))
   else if n↑.ltype = vectype then tp := tmake(niltrans,n↑.v)
   else tp := n↑.t;
  if b then relnode(n);
  tp := ttmul(tb,tp);			(* shift to proper coord sys *)
  if t <> nil then tp := ttmul(t,tp);
  getDepr := tp;
  end;

 procedure getCode(s: statementp);
  var e: enventryp;
  begin
  if s = nil then e := nil
   else
    begin
    with s↑ do
     if stype = signaltype then e := gtVarn(event)
      else e := gtVarn(oncond);
    msg↑.evt := e↑.evt;			(* event to signal for code *)
    msg↑.bits := msg↑.bits + CODECB;
    end;
  end;

 procedure doMode1;
  var i: integer;
  begin
  with curInt↑ do 
    begin			(* set up force system, enable all cmons *)
    e := gtVarn(spc↑.cf);		(* remember what we're moving *)
    mech := e↑.f;
    if mech↑.ftype then			(* check it's a device *)
     if mech↑.dev = nil then
      begin			(* yow! frame that's not affixed to an arm *)
      pp20L('Control frame not af',20); pp20('fixed to any device:',20);
      pp20(' Assuming barm      ',14); ppLine;
      end;
    ffr := nil;
    stiff := nil;
    gather := nil;
    zwrist := nil;
    cmForce := 0;
    useForce := 0;
    cl := spc↑.clauses;
    while cl <> nil do			(* run through clauses *)
     with cl↑ do
      begin
      if ntype = ffnode then ffr := cl
       else if ntype = stiffnode then stiff := cl
       else if ntype = gathernode then gather := cl
       else if ntype = wristnode then zwrist := cl
       else if ntype = forcenode then useForce := useForce + 1
       else if ntype = cmonnode then
	if cmon↑.oncond↑.ntype = forcenode then cmForce := cmForce + 1;
      cl := next;
      end;

    if (ffr <> nil) or (cmForce + useForce > 0) or (gather <> nil) then
      begin
      msg↑.cmd := setccmd;
  (* deal with which arm here *) fbits := BLUARM; (* but for now... *)
      if ffr <> nil then
	begin
	val := getNval(ffr↑.ff,b);	(* get force frame value *)
	if ffr↑.csys then fbits := fbits + FTABLE;
	msg↑.bits := fbits;
	sendTrans(val↑.t);		(* send command & trans over *)
	if b then relNode(val);
	end
       else 
	begin
	msg↑.bits := fbits + FTABLE;
	sendTrans(niltrans);		(* send command & trans over *)
	end;
      signalArm;			(* wake up ARM servo background job *)
      end;

    if zwrist <> nil then b := zwrist↑.notp
     else if (ffr <> nil) or (stiff <> nil) then b := true
     else b := false;
    if b then
      begin
      msg↑.cmd := zerowristcmd;		(* tell arm servo to zero wrist *)
      sendCmd;
      end;

    if stiff <> nil then
      begin
      val1 := getNval(stiff↑.fv,b1);			(* get force vector *)
      val2 := getNval(stiff↑.mv,b2);			(* get moment vector *)
      if stiff↑.coc <> nil then
	begin
	val := getNval(stiff↑.coc,b);			(* get coc value *)
	t := val↑.t;
	end
       else begin t := niltrans; b := false end;
      with msg↑ do
       begin
       cmd := setstiffcmd;
       for i := 1 to 3 do
	begin
	t[i] := val1↑.v↑.val[i];
	t[i+3] := val2↑.v↑.val[i];
	end;
       end;
      sendTrans(t);			(* send stiffnesses & coc trans over *)
      signalArm;			(* wake up ARM servo background job *)
      if b1 then killNode(val1);
      if b2 then killNode(val2);
      if b then relNode(val);
      end
     else if useForce > 0 then
      begin				(* add default stiffness *)
      with msg↑ do
       begin
       cmd := setstiffcmd;
       for i := 1 to 3 do
	begin
	t[i] := 40;
	t[i+3] := 100;
	end;
       end;
      sendTrans(niltrans);		(* send stiffnesses & coc trans over *)
      signalArm;			(* wake up ARM servo background job *)
      end;

    if gather <> nil then
      begin
  (* deal with which arm here someday *)
      with msg↑ do
       begin
       cmd := gathercmd;
       bits := gather↑.gbits;
       end;
      sendCmd;				(* send gather command over *)
      end;

    if ffr <> nil then			(* no bias forces if no force frame *)
      begin
      cl := spc↑.clauses;
      while cl <> nil do			(* run through clauses *)
       begin
       with cl↑ do
	if ntype = forcenode then		(* check for bias forces *)
	  begin
	  val := getNval(cl↑.fval,b);		(* get force magnitude *)
	  r := val↑.s;
	  if b then relnode(val);
	  fbits := m1forcebits(cl,b);
	  if b then r := -r;
  (* deal with which arm here *) fbits := fbits + BLUARM; (* but for now... *)
	  with msg↑ do
	   begin
	   cmd := biasoncmd;
	   bits := fbits;
	   mag := r;
	   end;
	  sendCmd;				(* tell arm about bias force *)
	  end;
       cl := cl↑.next;
       end;
      end;
    m1MvStart;		(* enable all condition monitors for move *)
    mode := 2;
    end;
  end {doMode1};

 procedure doMode2;
  begin
  with curInt↑ do 
    begin		(* set up motion specs for arm code & send it over *)
    ev := getEvent;	(* event to use for signalling when motion finishes *)
    ev↑.count := -1;
    ev↑.waitlist := curInt;
    mechbits := m1getMechbits;
    nsegs := 0;
    if mech↑.ftype then
      if mech↑.dev <> nil then fr := mech↑.dev	(* get frame for device *)
       else fr := barm
     else fr := mech;

    nulling := true;			(* no nulling is the default *)
    dest := nil;
    wobble := nil;
    sfac := nil;
    dur := nil;
    arrv := nil;
    appr := nil;
    depr := nil;
    apprp := true;			(* assume default approach *)
    deprp := fr↑.depr <> nil;	(* default departure if last had approach *)
    cl := spc↑.clauses;
    while cl <> nil do			(* run through clauses *)
     with cl↑ do
      begin
      if ntype = destnode then begin dest := cl; nsegs := nsegs + 1 end
       else if ntype = wobblenode then wobble := cl
       else if ntype = sfacnode then sfac := cl
       else if ntype = durnode then dur := cl
       else if ntype = nullingnode then nulling := notp
       else if ntype = apprnode then
	 begin
	 appr := cl;
	 if loc = nil then apprp := false	(* approach = nildeproach *)
	  else begin apprp := true; nsegs := nsegs + 1 end
	 end
       else if ntype = deprnode then
	 begin
	 depr := cl;
	 if loc = nil then deprp := false	(* departure = nildeproach *)
	  else begin deprp := true; nsegs := nsegs + 1 end
	 end
       else if ntype = viaptnode then nsegs := nsegs + 1
       else if ntype = cmonnode then
	 begin
	 if cmon↑.oncond↑.ntype = arrivalnode then arrv := cmon↑.oncond;
	 end;
      cl := next;
      end;

    if deprp or mech↑.ftype then
      tb := feval(mech);		(* get current cf position *)
    if deprp then
      if depr <> nil then		(* explicit departure point? *)
	tb↑.refcnt := tb↑.refcnt + 1	(* need it to compute departure *)
       else nsegs := nsegs + 1;		(* add in default departure seg *)
    if apprp and (appr = nil) then	(* default approach point? *)
     with dest↑.loc↑ do
      if ((ntype = leafnode) and (ltype = varitype)) or
	 ((ntype = exprnode) and (op = arefop)) then
	nsegs := nsegs + 1		(* add in default approach seg *)
       else apprp := false;		(* don't want default approach *)
    if mech↑.ftype then
      begin				(* get offset trans to take cf to arm *)
      t := whereArm(mechbits);		(* Get current device pos *)
      t := ttmul(t,tinvrt(tb));		(* compute offset *)
      end
     else t := nil;				(* no offset needed *)

    with msg↑ do
     begin
     cmd := movehdrcmd;
     dev := mechbits;
     if nulling then bits := NULLINGCB else bits := 0;
     n := nsegs;
     evt := ev;
     end;

    if sfac <> nil then
      begin					(* use local speed factor *)
      val := getNval(sfac↑.clval,b);
      msg↑.sfac := val↑.s;
      if b then relnode(val);
      end
     else
      begin					(* use global speed factor *)
      msg↑.sfac := speedfactor↑.s;
      end;

    if dur <> nil then				(* duration *)
      begin
      val := getNval(dur↑.durval,b);
      msg↑.dur := val↑.s;
      if dur↑.durrel < seqop then i := DURLBCB
       else if dur↑.durrel > seqop then i := DURUBCB
       else i := DUREQCB;
      msg↑.bits := msg↑.bits + i;
      if b then relnode(val);
      end;

    if wobble <> nil then			(* wobble *)
      begin
      val := getNval(wobble↑.clval,b);
      msg↑.wobble := val↑.s;
      msg↑.bits := msg↑.bits + WOBBLECB;
      if b then relnode(val);
      end;

    sendCmd;			(* tell arm servo we're starting a motion *)

    msg↑.cmd := movesegcmd;		(* now get values for trajectory points *)

    if deprp then			(* departure: loc & event *)
      begin
      msg↑.bits := DEPRPTCB;
      if depr = nil then tl := fr↑.depr	(* default departure point *)
       else
	begin				(* explicit departure point *)
	n := getNval(depr↑.loc,b);
	tl := getDepr(n,b);
	tb↑.refcnt := tb↑.refcnt - 1;
	if tb↑.refcnt <= 0 then relTrans(tb);	(* done with it now *)
	getCode(depr↑.code);
	end;
      sendTrans(tl);
      end;

    cl := spc↑.clauses;
    while cl <> nil do			(* run through clauses *)
     begin
     with cl↑ do
      if ntype = viaptnode then		(* vias: loc, duration, velocity & event *)
	begin
	msg↑.bits := VIAPTCB;
	tl := getLoc(via);
	if duration <> nil then
	  begin
	  val := getNval(duration↑.durval,b);
	  msg↑.dur := val↑.s;
	  if duration↑.durrel < seqop then i := DURLBCB
	   else if duration↑.durrel > seqop then i := DURUBCB
	   else i := DUREQCB;
	  msg↑.bits := msg↑.bits + i;
	  if b then relnode(val);
	  end;
	if velocity <> nil then
	  begin
	  val := getNval(velocity,b);
	  msg↑.bits := msg↑.bits + VELOCCB;
	  with val↑.v↑ do
	   begin
	   msg↑.v1 := val[1];
	   msg↑.v2 := val[2];
	   msg↑.v3 := val[3];
	   end;
	  if b then relnode(val);
	  end;
	getCode(cl↑.vcode);
	sendTrans(tl);
	end;
     cl := cl↑.next;
     end;

    if apprp then			(* approach: loc & event *)
      begin
      msg↑.bits := APPRPTCB;
      if appr <> nil then
	begin				(* explicit approach point *)
	n := getNval(appr↑.loc,b);
	getCode(appr↑.code);
	end;
      tb := getLoc(dest↑.loc);		(* need to get destination location *)
      tb↑.refcnt := tb↑.refcnt + 1;	(* make sure we keep it for later *)
      if appr <> nil then tl := getDepr(n,b)	(* explicit approach point *)
       else
	begin				(* default appoach point *)
	tl := tvadd(tb,svmul(3,zhat));
	if t <> nil then tl := ttmul(t,tl);
	end;
      tb↑.refcnt := tb↑.refcnt - 1;
      upTrans(fr↑.appr,tl);		(* save it for next motion *)
      sendTrans(tl);
      end
     else
      begin
      tb := getLoc(dest↑.loc);	(* get destination for below *)
      upTrans(fr↑.appr,nil);	(* remember no default depr for next motion *)
      end;
				(* destination: loc & event *)
    uptrans(fr↑.tdest,tb);		(* make a copy of dest for later use *)
    msg↑.bits := DESTPTCB;
    if arrv <> nil then
      begin
      with arrv↑.evar↑ do e := getVar(level,offset);
      msg↑.evt := e↑.evt;		(* event to signal for code *)
      msg↑.bits := msg↑.bits + CODECB;
      end;
    sendTrans(tb);

    signalArm;			(* finally let background job deal with traj *)
    mode := 3;
    curInt↑.status := devicewait; 	(* don't for simulation version *)
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;
  end {doMode2};

 begin {doMove - main body}
 with curInt↑ do
  begin
  st := spc;			(* remember MOVE statement *)
  case mode of
1:  doMode1;

2:  doMode2;

3:  m1MvEnd;		(* do end of motion cleanup, run error handler, etc. *)

4:  m1MvRetry;		(* deal with user response if there was an error *)

  end;

  if curInt <> nil then	(* in case we're waiting for an error response *)
    if spc = st↑.next then
      begin			(* doesn't appear to have been any errors *)
      if mech↑.ftype then			(* get frame for device *)
	if mech↑.dev <> nil then fr := mech↑.dev
	 else fr := barm
       else fr := mech;
      upTrans(fr↑.depr,fr↑.appr);	(* update default departure point *)
      end;
  end;
 end;